/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2018 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#include <MMM/FactoryPluginLoader.h>
#include <MMM/Exceptions.h>
#include "MMMDataGloveConverterConfiguration.h"
#include "DataGloveConverter.h"
#include <MMM/Motion/MotionWriterXML.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include "DummyMotion.h"

int main(int argc, char *argv[])
{
    MMM_INFO << "--- MMM DATA GLOVE CONVERTER ---" << std::endl;

    MMMDataGloveConverterConfiguration *configuration = new MMMDataGloveConverterConfiguration();
    if (!configuration->processCommandLine(argc,argv)) {
        MMM_ERROR << "Could not process command line, aborting." << std::endl;
        return -1;
    }

    try {
        MMM::DataGloveConverter* converter = new MMM::DataGloveConverter(configuration->dataGloveConfigPath);
        MMM::KinematicSensorPtr sensor = converter->convert(configuration->dataGloveDataPath);
        if (configuration->testOnly) {
            MMM_INFO << "Test only mode activated!" << std::endl;

            MMM::MotionPtr motion(new MMM::DummyMotion());
            motion->addSensor(sensor);

            MMM_INFO << "Writing motion to " << configuration->outputMotionPath << std::endl;
            MMM::MotionWriterXML::writeMotion(motion, configuration->outputMotionPath);
        } else {
            MMM::MotionReaderXMLPtr motionReader(new MMM::MotionReaderXML(configuration->sensorPluginPaths));
            MMM_INFO << "Reading motion file '" << configuration->inputMotionPath << "'!" << std::endl;
            MMM::MotionList motions = motionReader->loadAllMotions(configuration->inputMotionPath);
            MMM::MotionPtr motion = MMM::Motion::getMotion(motions, configuration->motionName);
            motion->addSensor(sensor);

            MMM_INFO << "Writing motions to " << configuration->outputMotionPath << std::endl;
            MMM::MotionWriterXML::writeMotion(motions, configuration->outputMotionPath);
        }

        MMM_INFO << "--- END ---" << std::endl;
        return 0;
    }
    catch (MMM::Exception::MMMException &e) {
        MMM_ERROR << e.what() << std::endl;
        return -2;
    }
}
